package com.grt192.mechanism;

import com.grt192.actuator.create.CreateDrive;
import com.grt192.core.CommandArray;
import com.grt192.core.Mechanism;
import com.grt192.sensor.create.EncoderAngle;
import com.grt192.sensor.create.EncoderVelocity;
import com.iaroc.irobot.IRobotCreate;

/**
 * A CreateBase manages IRobotCreate drive function
 * 
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class CreateBase extends Mechanism {

	public static final int MAX_VELOCITY = 500;
	private final IRobotCreate create;
	private CreateDrive dt;
	private EncoderAngle angleEncoder;
	private EncoderVelocity velocityEncoder;

	public CreateBase(IRobotCreate create, CreateDrive dt) {
		this.create = create;
		this.dt = dt;
	}

	/**
	 * Drive by commanding each wheel's motorspeed
	 * 
	 * @param left
	 *            speed of left wheel in mm/s [-500...500]
	 * @param right
	 *            speed of right wheel in mm/s [-500...500]
	 */
	public void velocityDrive(double left, double right) {
		dt.setControlMode(CreateDrive.CONTROL_DIRECT);
		dt.enqueueCommand(new CommandArray(left, right));
	}

	/**
	 * Drive by commanding each wheel's percent of maximum speed
	 * 
	 * @param left
	 *            % of max velocity [-1...1]
	 * @param right
	 *            % of max velocity [-1...1]
	 */
	public void percentDrive(double left, double right) {
		dt.setControlMode(CreateDrive.CONTROL_PERCENT);
		dt.enqueueFreeCommand(new CommandArray(left, right));
	}

	/**
	 * 
	 * @return
	 */
	synchronized public CreateDrive getDT() {
		// if (dt == null) {
		// dt = new CreateDrive(create);
		// dt.start();
		// }
		return dt;
	}

	/**
	 * 
	 * @return
	 */
	synchronized public EncoderAngle getAngleEncoder() {
		if (angleEncoder == null) {
			angleEncoder = new EncoderAngle(create, 50, "EncoderAngle");
			angleEncoder.start();
		}
		return angleEncoder;
	}

	/**
	 * 
	 * @return
	 */
	synchronized public EncoderVelocity getVelocityEncoder() {
		if (velocityEncoder == null) {
			velocityEncoder = new EncoderVelocity(create, 100,
					"EncoderVelocity");
			velocityEncoder.start();
		}
		return velocityEncoder;
	}
}
